#pragma once

#include <iostream>
#include <algorithm>
#include <queue>
#include <mutex>
#include <thread>
#include <vector>
#include <memory>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Imu.h>

#include <parameter.hpp>
#include <front_end.hpp>


// 一个run的单例

extern std::mutex g_picLeftLock;
extern std::queue<sensor_msgs::ImageConstPtr> g_picLeftQue;
extern std::mutex g_picRightLock;
extern std::queue<sensor_msgs::ImageConstPtr> g_picRightQue;



class run
{
private:
    ros::NodeHandle m_nh;
    std::shared_ptr<ReadParameter> m_param; 
    ros::Subscriber leftImage;
    ros::Subscriber rightImage;

    FeatureTracker* mpImageTracker;
    std::thread* mpImageTrackerThread;
public:

    run(ros::NodeHandle &n, std::shared_ptr<ReadParameter> inParam):m_nh(n){
        // sub topic
        m_param = std::shared_ptr<ReadParameter>(inParam);
        leftImage = m_nh.subscribe(m_param->image1_topic, 10,  &run::steroLeftImgeCallback, this);
        rightImage = m_nh.subscribe(m_param->image2_topic, 10,  &run::steroRightImgeCallback, this);
    }
    
    ~run(){

    }
    void runBuildMap(void){
        // run front end
        mpImageTracker = new FeatureTracker(m_param);			//参数
        mpImageTrackerThread = new std::thread(&FeatureTracker::run,	//线程的主函数
    							           mpImageTracker);					//该函数的参数
        // run back end
    }
    void runLocationFromMap(void){
        // run front end

        // run location
    }

    // 图像left单回调函数
    void steroLeftImgeCallback(const sensor_msgs::ImageConstPtr& img_msg)
    {
        g_picLeftLock.lock();
        g_picLeftQue.push(img_msg);
        g_picLeftLock.unlock();
    }
    // 图像right单回调函数
    void steroRightImgeCallback(const sensor_msgs::ImageConstPtr& img_msg)
    {
        g_picRightLock.lock();
        g_picRightQue.push(img_msg);
        g_picRightLock.unlock();
    }

    // 读取groundtruth
    //void 
};

